#include "skeleton.h"

using namespace krryn::animation;
using namespace krryn::math;

skeleton::skeleton(unsigned int a_NumJoints) : m_NumJoints(a_NumJoints){
	size_t a_Count = sizeof(float) * num_joints_rounded();

	m_Parents = (unsigned int*)_aligned_malloc(a_Count, 16);

	m_X = (__m128*)_aligned_malloc(a_Count, 16);
	m_Y = (__m128*)_aligned_malloc(a_Count, 16);
	m_Z = (__m128*)_aligned_malloc(a_Count, 16);

	m_RX = (__m128*)_aligned_malloc(a_Count, 16);
	m_RY = (__m128*)_aligned_malloc(a_Count, 16);
	m_RZ = (__m128*)_aligned_malloc(a_Count, 16);
	m_RW = (__m128*)_aligned_malloc(a_Count, 16);
}

skeleton::~skeleton(){
	_aligned_free(m_Parents);
 
	_aligned_free(m_X);
	_aligned_free(m_Y);
	_aligned_free(m_Z);

	_aligned_free(m_RX);
	_aligned_free(m_RY);
	_aligned_free(m_RZ);
	_aligned_free(m_RW);
}

unsigned int skeleton::num_joints() const { return m_NumJoints; }

void skeleton::set_joint_parent(unsigned int a_Idx, unsigned int a_Parent){
	m_Parents[a_Idx] = a_Parent;
}

void skeleton::set_joint_position(unsigned int a_Idx, vector3f a_Position){
	((float*)m_X)[a_Idx] = a_Position.m_X;
	((float*)m_Y)[a_Idx] = a_Position.m_Y;
	((float*)m_Z)[a_Idx] = a_Position.m_Z;
}

void skeleton::set_joint_rotation(unsigned int a_Idx, quaternionf a_Rotation){
	((float*)m_RX)[a_Idx] = a_Rotation.m_X;
	((float*)m_RY)[a_Idx] = a_Rotation.m_Y;
	((float*)m_RZ)[a_Idx] = a_Rotation.m_Z;
	((float*)m_RW)[a_Idx] = a_Rotation.m_W;
}

unsigned int skeleton::get_joint_parent(unsigned int a_Idx){
	return m_Parents[a_Idx];
}

vector3f skeleton::get_joint_position(unsigned int a_Idx){
	return vector3f(
		((float*)m_X)[a_Idx],
		((float*)m_Y)[a_Idx],
		((float*)m_Z)[a_Idx]);
}

quaternionf skeleton::get_joint_rotation(unsigned int a_Idx){
	return quaternionf(
		((float*)m_RX)[a_Idx],
		((float*)m_RY)[a_Idx],
		((float*)m_RZ)[a_Idx],
		((float*)m_RW)[a_Idx]);
}

void skeleton::interpolate(skeleton *a_Result, skeleton *a_First, skeleton *a_Second, float a_Alpha){
	std::cout << a_Alpha << std::endl;
	__m128 l_Alpha4 = _mm_set_ps1(a_Alpha);

	unsigned int a_SimdCount = a_Result->num_joints_rounded() >> 2;

#define sse_lerp(y, x, a) x//_mm_add_ps(y, _mm_mul_ps(_mm_sub_ps(x, y), a));
	// TODO: profile if two seperate loops are faster (eg. one lerp, and one normalize step)
	// TODO: prefetching?

	// TODO: might want to use SSE memcpy to copy parents
	for(unsigned int i = 0; i < a_Result->num_joints(); i++)
		a_Result->m_Parents[i] = a_First->m_Parents[i];

	// Perform linear interpolation for positions
	// Perform normalized linear interpolation for quaterions (nlerp)
	for(unsigned int i = 0; i < a_SimdCount; i++){
		a_Result->m_X[i] = sse_lerp(a_First->m_X[i], a_Second->m_X[i], l_Alpha4);
		a_Result->m_Y[i] = sse_lerp(a_First->m_Y[i], a_Second->m_Y[i], l_Alpha4);
		a_Result->m_Z[i] = sse_lerp(a_First->m_Z[i], a_Second->m_Z[i], l_Alpha4);
/*
		a_Result->m_RX[i] = sse_lerp(a_First->m_RX[i], a_Second->m_RX[i], l_Alpha4);
		a_Result->m_RY[i] = sse_lerp(a_First->m_RY[i], a_Second->m_RY[i], l_Alpha4);
		a_Result->m_RZ[i] = sse_lerp(a_First->m_RZ[i], a_Second->m_RZ[i], l_Alpha4);
		a_Result->m_RW[i] = sse_lerp(a_First->m_RW[i], a_Second->m_RW[i], l_Alpha4);*/
#if TODO_BENCHMARK_THIS
	}

	for(unsigned int i = 0; i < a_SimdCount; i++){
#endif
		// Normalize quaternions
	/*	__m128 l_Length = _mm_add_ps(
			_mm_add_ps(_mm_mul_ps(a_Result->m_RX[i], a_Result->m_RX[i]), _mm_mul_ps(a_Result->m_RY[i], a_Result->m_RY[i])),
			_mm_add_ps(_mm_mul_ps(a_Result->m_RZ[i], a_Result->m_RZ[i]), _mm_mul_ps(a_Result->m_RW[i], a_Result->m_RW[i]))
		);

		__m128 l_Norm = _mm_rsqrt_ps(l_Length);

		a_Result->m_RX[i] = _mm_mul_ps(a_Result->m_RX[i], l_Norm);
		a_Result->m_RY[i] = _mm_mul_ps(a_Result->m_RY[i], l_Norm);
		a_Result->m_RZ[i] = _mm_mul_ps(a_Result->m_RZ[i], l_Norm);
		a_Result->m_RW[i] = _mm_mul_ps(a_Result->m_RW[i], l_Norm);*/
	}

#undef sse_lerp
}

unsigned int skeleton::num_joints_rounded() const {
	return (m_NumJoints + 3) & ~3;
}